home *** CD-ROM | disk | FTP | other *** search
/ Amiga Plus Leser 15 / Amiga Plus Leser CD 15.iso / Tools / Development / mmu / MuManual / C_Sources / FPSPSnoopHandler.asm < prev    next >
Assembly Source File  |  2002-03-12  |  12KB  |  363 lines
  1.         opt o+,ow-
  2. ;*************************************************
  3. ;** FPSPSnoop                                   **
  4. ;**                                             **
  5. ;** the handler part: Detects to be emulated    **
  6. ;** instructions                                **
  7. ;**                                             **
  8. ;** Version 40.3 © Thomas Richter               **
  9. ;** THOR Software, 30.8.2001                    **
  10. ;*************************************************
  11.  
  12.         opt c+
  13.  
  14.         include inc:macros.asm
  15.  
  16. RawIOPutChar=         -516          ;this one is undocumented, but there.
  17. Supervisor=             -30
  18.  
  19. ;FOLD Register-Setup
  20. ;*************************************************
  21. ;** Register-Setup                              **
  22. ;** describes the register state at the         **
  23. ;** exception time. Will be filled in here      **
  24. ;** on the stack                                **
  25. ;*************************************************
  26.         rsreset
  27. rs_DataRegs:            rs.l 8          ;d0-d7
  28. rs_AddrRegs:            rs.l 8          ;a0-a7 (USP is a7)
  29. rs_SSP:                 rs.l 1          ;SSP
  30. rs_PC:                  rs.l 1          ;PC of the faulting instruction
  31. rs_SR:                  rs.w 1          ;Status register
  32. rs_Vector:              rs.w 1          ;Vector offset
  33. rs_EA:                  rs.l 1          ;EA of faulting instruction, if available
  34. rs_FPURegs:             rs.b 12*8       ;fp0-fp7
  35. rs_FPCR:                rs.l 1          ;FPCR
  36. rs_FPIAR:               rs.l 1          ;FPIAR
  37. rs_FPSR:                rs.l 1          ;FPSR
  38. rs_len:                 rs.b 0
  39. rs_FPUState:            rs.l 1          ;FPU status indicator here
  40. rs_FPUFrame:            rs.b 0          ;FPU stack frame here
  41. ;ENDFOLD
  42.  
  43.  
  44.         machine mc68040
  45.  
  46.         xref _Exception_Handler
  47.         xref _OldVectors
  48.         xref _SysBase
  49.  
  50.  
  51. ;FOLD GetVBR
  52.         xdef _GetVBR
  53. _GetVBR:
  54.         saveregs a5-a6
  55.         move.l _SysBase(a4),a6
  56.         lea _ReadVBR(pc),a5
  57.         jsr Supervisor(a6)
  58.         loadregs
  59.         rts
  60. _ReadVBR:
  61.         movec.l vbr,d0
  62.         rte
  63.  
  64.         
  65. ;FOLD Exception_Callin
  66. ;*************************************************
  67. ;** Exception-Callin                            **
  68. ;*************************************************
  69.         xdef _Exception_Callin
  70. _Exception_Callin:
  71.         move.l a6,-(a7)
  72.         move.l a7,a6                            ;keep stack base here
  73.  
  74.         fsave -(a7)                             ;keep FPU state frame here
  75.         clr.l -(a7)                             ;restore FPU state flag indicator
  76.         reserve rs_len                          ;reserve registers
  77.  
  78.         movem.l d0-d7/a0-a5,rs_DataRegs(a7)     ;keep data regs
  79.  
  80.         fmovem.l fpcr,rs_FPCR(a7)
  81.         fmovem.l fpiar,rs_FPIAR(a7)
  82.         fmovem.l fpsr,rs_FPSR(a7)
  83.         fmovem.x fp0-fp7,rs_FPURegs(a7)
  84.  
  85.         movec.l usp,a0
  86.         move.l (a6),4*6+rs_AddrRegs(a7)         ;keep a6
  87.         move.l a0,4*7+rs_AddrRegs(a7)           ;keep USP
  88.         move.l a6,a1                            ;keep SSP
  89.         move.w 4(a6),rs_SR(a7)                  ;keep status register
  90.         move.w 4+6(a6),d0                       ;get vector offset
  91.         clr.l rs_EA(a7)                         ;clear EA
  92.         move.w d0,d1
  93.         and.w #$f000,d0                         ;extract stack frame type
  94.         and.w #$0fff,d1                         ;extract vector offset from here
  95.         move.w d1,rs_Vector(a7)                 ;store here
  96.         rol.w #4,d0                             ;get offset
  97.  
  98.         cmp.w #$f4,d1                           ;is it unimp integer?
  99.         beq.s .isinteger
  100. ;
  101. ; ok, and now for the hacky stuff. Problem is as follows: if the FPU status
  102. ; is null, and we run into the frestore below, the fpiar will be null-ed as
  103. ; well such that the real exception handler will be unable to fetch the
  104. ; instruction. Therefore, we have to modify a NULL stackframe to an IDLE
  105. ; stackframe.
  106. ;
  107.         move.l _SysBase,a0                      ;check whether this is a 040 or a 060
  108.         btst #7,$129(a0)                        ;060?
  109.         bne.s .is060
  110.  
  111.         tst.b rs_FPUFrame(a7)                   ;NULL-State?
  112.         bra.s .check
  113. .is060:
  114.         tst.b 2+rs_FPUFrame(a7)                 ;NULL-State?
  115. .check:
  116.         bne.s .isinteger
  117.         move.l a6,rs_FPUState(a7)               ;leave FPU frame untouched
  118. .isinteger:
  119.  
  120.         addq.l #4,a1                            ;compensate for the a6-push
  121.         move.l 4+2(a6),rs_PC(a7)                ;store the PC here, as first approximation
  122.         addq.l #8,a1                            ;compensate for short stack frame type
  123.         tst.b d0                                ;frame #0: PC is correct and points to the FPU instruction
  124.         beq.s .int2
  125.         cmp.b #2,d0                             ;Post-Instruction, or UnimpFPU?
  126.         beq.s .longframe
  127.         cmp.b #3,d0
  128.         bne.s .pcisfine
  129. .longframe:
  130.         addq.l #4,a1                            ;enlarge the stack frame
  131.         move.l 4+8(a6),rs_EA(a7)                ;fill in EA
  132. .pcisfine:
  133.  
  134.         cmp.w #$f4,d1                           ;is it unimp integer?
  135.         beq.s .int2
  136.         move.l rs_FPIAR(a7),rs_PC(a7)           ;use this as PC
  137. .int2:
  138.  
  139.         move.l a7,a0                            ;return data here
  140.         move.l a1,rs_SSP(a7)                    ;keep SSP
  141.         bsr _Exception_Handler
  142.  
  143.         ;** Now perform state restauration
  144.  
  145.         lea _OldVectors,a0                      ;get me
  146.         move.w rs_Vector(a7),d0                 ;read offset
  147.         move.l (a0,d0.w),a0                     ;read old jump-in
  148.  
  149.         move.l (a6),a1                          ;restore a6->a1
  150.         move.l a0,(a6)                          ;push as jump-in
  151.         move.l a1,a6                            ;restore a6
  152.  
  153.         movem.l rs_DataRegs(a7),d0-d1           ;restore d0,d1
  154.         movem.l rs_AddrRegs(a7),a0-a1           ;restore a0,a1
  155.                                                 ;FPU frame is not touched here!
  156.         fmovem.l rs_FPSR(a7),fpsr
  157.         fmovem.l rs_FPIAR(a7),fpiar
  158.         fmovem.l rs_FPCR(a7),fpcr
  159.  
  160.         restore
  161.         tst.l (a7)                              ;restore FPU state?
  162.         beq.s .restore
  163.         move.l (a7),a7                          ;otherwise, pop SP
  164.         rts
  165.  
  166. .restore:
  167.         addq.l #4,a7                            ;pop the status indicator
  168.         frestore (a7)+                          ;restore FPU state frame
  169.  
  170.         rts                                     ;run into the destination setup by the old vector
  171. ;ENDFOLD
  172. ;FOLD PrintFmt
  173. ;*************************************************
  174. ;** PrintFmt                                    **
  175. ;** Print a formatted string *a0 with stream    **
  176. ;** in *a1 to the serial port. Output can be    **
  177. ;** captured with Sushi or Sashimi.             **
  178. ;** *a6 = SysBase                               **
  179. ;**                                             **
  180. ;** all entries in the stream are longs, but    **
  181. ;** they are used as follows:                   **
  182. ;**             %d      signed decimal          **
  183. ;**             %x      unsigned hex            **
  184. ;**             %s      string                  **
  185. ;**             %c      one character           **
  186. ;**             %%      the % sign itself       **
  187. ;*************************************************
  188.         xdef _VPrintFmt
  189. _VPrintFmt:
  190.         saveregs a2-a3
  191.  
  192.         move.l a0,a2                    ;get format string
  193.         move.l a1,a3                    ;keep stream
  194.  
  195.         do
  196.          move.b (a2)+,d0                ;next character
  197.          break.s eq                     ;abort if done
  198.          cmp.b #'%',d0                  ;the format character?
  199.          bne.s .putchar
  200.  
  201.          move.b (a2)+,d0                ;get the format type
  202.          break.s eq                     ;abort if done
  203.          cmp.b #'%',d0                  ;is it % itself?
  204.          beq.s .putchar
  205.          cmp.b #'d',d0                  ;decimal?
  206.          bne.s .nodecimal
  207.          move.l (a3)+,d0                ;get the number
  208.          bsr _PutDecimal
  209.          reloop.s
  210. .nodecimal:
  211.          cmp.b #'x',d0                  ;hex?
  212.          bne.s .nohex
  213.          move.l (a3)+,d0                ;get hex number
  214.          bsr _PutHex
  215.          reloop.s
  216. .nohex:
  217.          cmp.b #'b',d0                  ;hex byte?
  218.          bne.s .nobyte
  219.          move.l (a3)+,d0
  220.          bsr _PutByte
  221.          reloop.s
  222. .nobyte:
  223.          cmp.b #'c',d0                  ;char?
  224.          bne.s .nochar
  225.          move.l (a3)+,d0
  226.          bra.s .putchar
  227. .nochar:
  228.          cmp.b #'s',d0                  ;string?
  229.          reloop.s ne                    ;if not, ignore
  230.  
  231.          move.l (a3)+,a0                ;get string
  232.          bsr _PutString
  233.          reloop.s
  234.  
  235. .putchar:
  236.          jsr RawIOPutChar(a6)        ;if not, just use the serial port
  237.         loop.s
  238.  
  239.         loadregs
  240.         rts
  241.  
  242. ;**
  243. ;** _PutHex:    Dump the hex number d0
  244. ;**
  245. _PutHex:
  246.         saveregs d2-d3
  247.         move.l d0,d3                    ;keep it
  248.  
  249.         for.l #8,d2
  250.          rol.l #4,d3                    ;next digit
  251.          move.b d3,d0                   ;to d0
  252.          and.b #$0f,d0                  ;mask nibble
  253.          or.b #'0',d0                   ;to ASCII
  254.          cmp.b #'9',d0                  ;a number 'a' to 'f' ?
  255.          bls.s .putme
  256.          add.b #'a'-'9'-1,d0            ;offset
  257. .putme:
  258.          jsr RawIOPutChar(a6)        ;if not, just use the serial port
  259.         next d2
  260.  
  261.         loadregs
  262.         rts
  263. ;**
  264. ;** _PutByte:    Dump the hex byte d0
  265. ;**
  266. _PutByte:
  267.         saveregs d2-d3
  268.         move.l d0,d3                    ;keep it
  269.  
  270.         for.l #2,d2
  271.          rol.b #4,d3                    ;next digit
  272.          move.b d3,d0                   ;to d0
  273.          and.b #$0f,d0                  ;mask nibble
  274.          or.b #'0',d0                   ;to ASCII
  275.          cmp.b #'9',d0                  ;a number 'a' to 'f' ?
  276.          bls.s .putme
  277.          add.b #'a'-'9'-1,d0            ;offset
  278. .putme:
  279.          jsr RawIOPutChar(a6)        ;if not, just use the serial port
  280.         next d2
  281.  
  282.         loadregs
  283.         rts
  284. ;**
  285. ;** _PutDecimal
  286. ;**
  287. _PutDecimal:
  288.         saveregs d2-d3/a2
  289.  
  290.         move.l d0,d3                    ;negative ?
  291.         beq.s .isnull                   ;or null ?
  292.         bpl.s .positive
  293.         moveq #'-',d0
  294.         neg.l d3
  295.         jsr RawIOPutChar(a6)            ;if not, just use the serial port
  296.         bra.s .positive
  297. .isnull:
  298.         moveq #'0',d0
  299.         jsr RawIOPutChar(a6)        ;if not, just use the serial port
  300.         bra.s .exit
  301.  
  302. .positive:
  303.         lea Powers(pc),a2               ;Powers of ten, lookup table
  304.         moveq #0,d2                     ;character used for the '0'.
  305.                                         ;Leading 0s are not printed
  306.         do
  307.          moveq #-1,d0                   ;divide d3 by d1, result -> d0
  308.          move.l (a2)+,d1                ;next smaller power
  309.          break.s eq                     ;we're done if we're done...
  310.          do
  311.           addq.l #1,d0                  ;this can loop at most 9 times
  312.           sub.l d1,d3                   ;childs stupid division algorithm
  313.          while.s cc                     ;until no more subtraction possible
  314.          add.l d1,d3                    ;undo the last operation
  315.                                         ;result in d0, remainder in d3
  316.          tst.b d0                       ;'0' or not ?
  317.          bne.s .nozero
  318.          tst.b d2                       ;a leading 0 ?
  319.          reloop.s eq                    ;if so, next digit
  320.          moveq #'0',d0                  ;if not, put a zero
  321.          bra.s .putit
  322. .nozero:
  323.          moveq #'0',d2                  ;next nuls are true nuls
  324.          or.b d2,d0                     ;to ASCII
  325. .putit:
  326.          jsr RawIOPutChar(a6)        ;if not, just use the serial port
  327.         loop.s
  328. .exit:
  329.         loadregs
  330.         rts
  331.  
  332. ;**
  333. ;** _PutString
  334. ;**
  335. _PutString:
  336.         saveregs a2
  337.  
  338.         move.l a0,a2
  339.         do
  340.          move.b (a2)+,d0
  341.          break.s eq
  342.          jsr RawIOPutChar(a6)        ;if not, just use the serial port
  343.         loop.s
  344.  
  345.         loadregs
  346.         rts
  347.  
  348. Powers:         ;** powers of ten for simple binary -> decimal conversion
  349.         dc.l 1000000000
  350.         dc.l 100000000
  351.         dc.l 10000000
  352.         dc.l 1000000
  353.         dc.l 100000
  354.         dc.l 10000
  355.         dc.l 1000
  356.         dc.l 100
  357.         dc.l 10
  358.         dc.l 1
  359.         dc.l 0          ;done.
  360.  
  361. ;ENDFOLD
  362.  
  363.